package behavior;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.subsumption.Behavior;

public class LeftWall implements Behavior {
	private UltrasonicSensor sonar;
	private boolean suppressed = false;

	public LeftWall(SensorPort port)
	{
		sonar = new UltrasonicSensor(port);
	}

	public boolean takeControl() {
		try {
			Thread.sleep(50);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		return ((sonar.getDistance() < 25) && (Motor.B.getTachoCount() < -5));
	}

	public void suppress() {
		suppressed = true;
	}

	public void action() {

		suppressed = false;
		LCD.clear();
		LCD.drawString("Front wall detected.", 0, 2);
		LCD.drawString("Waiting for confirmation...", 0, 3);

		try {
			Thread.sleep(2000);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}

		if(sonar.getDistance() < 25){
			LCD.clear(3);
			Motor.B.rotateTo(90, true);

			while( Motor.B.isMoving() && !suppressed )
				Thread.yield();

			Motor.B.stop();

			if(Motor.B.getTachoCount() < 80){
				while(Motor.B.getTachoCount() < 80){
					Motor.B.rotateTo(90, true);

					while( Motor.B.isMoving() && !suppressed )
						Thread.yield();
				}
				Motor.B.stop();
			}

		}
	}
}

